/*
  pmap: simple mapping utilities
  Copyright (C) 2004 Andrew Howard  ahoward@usc.edu

  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  as published by the Free Software Foundation; either version 2
  of the License, or (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/

/** @file slap.h 
    @brief Small Linear Algrebra Package (SLAP); this code is auto-generated. */
/* Desc: Small Linear Algrebra Package (SLAP) */
/* Author: Andrew Howard */
/* Date: Sun Dec 12 15:22:13 2004 */
/* Code generated by slap.py */
#include <assert.h>
#include <math.h>
#include "slap.h"

#if defined WIN32
  #include <float.h>
  #define isnan _isnan
#endif

/* Set vector elements. */
vector2_t vector2_set(double v0, double v1)
{
  vector2_t a;
  a.x = v0;
  a.y = v1;
  return a;
}

/* Add two vectors. */
vector2_t vector2_add(vector2_t a, vector2_t b)
{
  vector2_t c;
  c.x = a.x + b.x;
  c.y = a.y + b.y;
  return c;
}

/* Add Subtract vectors: c = a - b. */
vector2_t vector2_sub(vector2_t a, vector2_t b)
{
  vector2_t c;
  c.x = a.x - b.x;
  c.y = a.y - b.y;
  return c;
}

/* Return unit vector. */
vector2_t vector2_unit(vector2_t a)
{
  vector2_t b;
  b = vector2_scale(1.0 / vector2_mag(a), a);
  return b;
}

/* Scale vector: c = ab. */
vector2_t vector2_scale(double a, vector2_t b)
{
  vector2_t c;
  c.x = a * b.x;
  c.y = a * b.y;
  return c;
}

/* Dot product: c = a.b. */
double vector2_dot(vector2_t a, vector2_t b)
{
  double c;
  c = 0.0;
  c += a.x * b.x;
  c += a.y * b.y;
  return c;
}


/* Vector magnitude: b = |a|. */
double vector2_mag(vector2_t a)
{
  double b;
  b = 0.0;
  b += a.x * a.x;
  b += a.y * a.y;
  return sqrt(b);
}

/* Euclidian distance between points a and b. */
double vector2_dist(vector2_t a, vector2_t b)
{
  double c;
  c = 0;
  c += (a.x - b.x) * (a.x - b.x);
  c += (a.y - b.y) * (a.y - b.y);
  return sqrt(c);
}



/* Set vector elements. */
vector3_t vector3_set(double v0, double v1, double v2)
{
  vector3_t a;
  a.x = v0;
  a.y = v1;
  a.z = v2;
  return a;
}

/* Add two vectors. */
vector3_t vector3_add(vector3_t a, vector3_t b)
{
  vector3_t c;
  c.x = a.x + b.x;
  c.y = a.y + b.y;
  c.z = a.z + b.z;
  return c;
}

/* Add Subtract vectors: c = a - b. */
vector3_t vector3_sub(vector3_t a, vector3_t b)
{
  vector3_t c;
  c.x = a.x - b.x;
  c.y = a.y - b.y;
  c.z = a.z - b.z;
  return c;
}

/* Return unit vector. */
vector3_t vector3_unit(vector3_t a)
{
  vector3_t b;
  b = vector3_scale(1.0 / vector3_mag(a), a);
  return b;
}

/* Scale vector: c = ab. */
vector3_t vector3_scale(double a, vector3_t b)
{
  vector3_t c;
  c.x = a * b.x;
  c.y = a * b.y;
  c.z = a * b.z;
  return c;
}

/* Dot product: c = a.b. */
double vector3_dot(vector3_t a, vector3_t b)
{
  double c;
  c = 0.0;
  c += a.x * b.x;
  c += a.y * b.y;
  c += a.z * b.z;
  return c;
}

/* Cross product: c = a x b. */
vector3_t vector3_cross(vector3_t a, vector3_t b)
{
  vector3_t c;
  c.x =  a.y * b.z - a.z * b.y;
  c.y = -a.x * b.z + a.z * b.x;
  c.z =  a.x * b.y - a.y * b.x;
  return c;
}

/* Vector magnitude: b = |a|. */
double vector3_mag(vector3_t a)
{
  double b;
  b = 0.0;
  b += a.x * a.x;
  b += a.y * a.y;
  b += a.z * a.z;
  return sqrt(b);
}

/* Euclidian distance between points a and b. */
double vector3_dist(vector3_t a, vector3_t b)
{
  double c;
  c = 0;
  c += (a.x - b.x) * (a.x - b.x);
  c += (a.y - b.y) * (a.y - b.y);
  c += (a.z - b.z) * (a.z - b.z);
  return sqrt(c);
}



/* Set vector elements. */
vector4_t vector4_set(double v0, double v1, double v2, double v3)
{
  vector4_t a;
  a.x = v0;
  a.y = v1;
  a.z = v2;
  a.w = v3;
  return a;
}

/* Add two vectors. */
vector4_t vector4_add(vector4_t a, vector4_t b)
{
  vector4_t c;
  c.x = a.x + b.x;
  c.y = a.y + b.y;
  c.z = a.z + b.z;
  c.w = a.w + b.w;
  return c;
}

/* Add Subtract vectors: c = a - b. */
vector4_t vector4_sub(vector4_t a, vector4_t b)
{
  vector4_t c;
  c.x = a.x - b.x;
  c.y = a.y - b.y;
  c.z = a.z - b.z;
  c.w = a.w - b.w;
  return c;
}

/* Return unit vector. */
vector4_t vector4_unit(vector4_t a)
{
  vector4_t b;
  b = vector4_scale(1.0 / vector4_mag(a), a);
  return b;
}

/* Scale vector: c = ab. */
vector4_t vector4_scale(double a, vector4_t b)
{
  vector4_t c;
  c.x = a * b.x;
  c.y = a * b.y;
  c.z = a * b.z;
  c.w = a * b.w;
  return c;
}

/* Dot product: c = a.b. */
double vector4_dot(vector4_t a, vector4_t b)
{
  double c;
  c = 0.0;
  c += a.x * b.x;
  c += a.y * b.y;
  c += a.z * b.z;
  c += a.w * b.w;
  return c;
}


/* Vector magnitude: b = |a|. */
double vector4_mag(vector4_t a)
{
  double b;
  b = 0.0;
  b += a.x * a.x;
  b += a.y * a.y;
  b += a.z * a.z;
  b += a.w * a.w;
  return sqrt(b);
}

/* Euclidian distance between points a and b. */
double vector4_dist(vector4_t a, vector4_t b)
{
  double c;
  c = 0;
  c += (a.x - b.x) * (a.x - b.x);
  c += (a.y - b.y) * (a.y - b.y);
  c += (a.z - b.z) * (a.z - b.z);
  c += (a.w - b.w) * (a.w - b.w);
  return sqrt(c);
}



/* Set matrix elements. */
matrix33_t matrix33_set(double v00, double v01, double v02, double v10, double v11, double v12, double v20, double v21, double v22)
{
  matrix33_t a;
  a.m[0][0] = v00;
  a.m[0][1] = v01;
  a.m[0][2] = v02;
  a.m[1][0] = v10;
  a.m[1][1] = v11;
  a.m[1][2] = v12;
  a.m[2][0] = v20;
  a.m[2][1] = v21;
  a.m[2][2] = v22;
  return a;
}

/* Set matrix elements. */
matrix33_t matrix33_setv(double v[16])
{
  matrix33_t a;
  a.m[0][0] = v[0];
  a.m[0][1] = v[1];
  a.m[0][2] = v[2];
  a.m[1][0] = v[3];
  a.m[1][1] = v[4];
  a.m[1][2] = v[5];
  a.m[2][0] = v[6];
  a.m[2][1] = v[7];
  a.m[2][2] = v[8];
  return a;
}

/* Construct identity matrix. */
matrix33_t matrix33_ident()
{
  matrix33_t a;
  a.m[0][0] = 1;
  a.m[1][0] = 0;
  a.m[2][0] = 0;
  a.m[0][1] = 0;
  a.m[1][1] = 1;
  a.m[2][1] = 0;
  a.m[0][2] = 0;
  a.m[1][2] = 0;
  a.m[2][2] = 1;
  return a;
}

/* Add two matrixs. */
matrix33_t matrix33_add(matrix33_t a, matrix33_t b)
{
  matrix33_t c;
  c.m[0][0] = a.m[0][0] + b.m[0][0];
  c.m[0][1] = a.m[0][1] + b.m[0][1];
  c.m[0][2] = a.m[0][2] + b.m[0][2];
  c.m[1][0] = a.m[1][0] + b.m[1][0];
  c.m[1][1] = a.m[1][1] + b.m[1][1];
  c.m[1][2] = a.m[1][2] + b.m[1][2];
  c.m[2][0] = a.m[2][0] + b.m[2][0];
  c.m[2][1] = a.m[2][1] + b.m[2][1];
  c.m[2][2] = a.m[2][2] + b.m[2][2];
  return c;
}

/* Add Subtract matrixs: c = a - b. */
matrix33_t matrix33_sub(matrix33_t a, matrix33_t b)
{
  matrix33_t c;
  c.m[0][0] = a.m[0][0] - b.m[0][0];
  c.m[0][1] = a.m[0][1] - b.m[0][1];
  c.m[0][2] = a.m[0][2] - b.m[0][2];
  c.m[1][0] = a.m[1][0] - b.m[1][0];
  c.m[1][1] = a.m[1][1] - b.m[1][1];
  c.m[1][2] = a.m[1][2] - b.m[1][2];
  c.m[2][0] = a.m[2][0] - b.m[2][0];
  c.m[2][1] = a.m[2][1] - b.m[2][1];
  c.m[2][2] = a.m[2][2] - b.m[2][2];
  return c;
}

/* Matrix multiplication c = ab. */
matrix33_t matrix33_mul(matrix33_t a, matrix33_t b)
{
  matrix33_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
  c.m[0][2] = + a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
  c.m[1][2] = + a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
  c.m[2][2] = + a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];
  return c;
}

#if defined VECTOR3
/** @brief Matrix/vector multiplication c = ab. */
vector3_t matrix33_mul_3(matrix33_t a, vector3_t b)
{
  vector3_t c;
  c.x = + a.m[0][0] * b.x + a.m[0][1] * b.y + a.m[0][2] * b.z;
  c.y = + a.m[1][0] * b.x + a.m[1][1] * b.y + a.m[1][2] * b.z;
  c.z = + a.m[2][0] * b.x + a.m[2][1] * b.y + a.m[2][2] * b.z;
  return c;
}
#endif

#if defined MATRIX33
/* Transpose matrix. */
matrix33_t matrix33_trans(matrix33_t a)
{
  matrix33_t b;
  b.m[0][0] = a.m[0][0];
  b.m[1][0] = a.m[0][1];
  b.m[2][0] = a.m[0][2];
  b.m[0][1] = a.m[1][0];
  b.m[1][1] = a.m[1][1];
  b.m[2][1] = a.m[1][2];
  b.m[0][2] = a.m[2][0];
  b.m[1][2] = a.m[2][1];
  b.m[2][2] = a.m[2][2];
  return b;
}
#endif

/* Invert matrix. */
matrix33_t matrix33_inv(matrix33_t a)
{
  matrix33_t b;
  double det;
  det = ( + a.m[0][0] * (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1]) - a.m[0][1] * (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0]) + a.m[0][2] * (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0]));
  b.m[0][0] = ( + (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1])) / det;
  b.m[0][1] = ( - (a.m[0][1] * a.m[2][2] - a.m[0][2] * a.m[2][1])) / det;
  b.m[0][2] = ( + (a.m[0][1] * a.m[1][2] - a.m[0][2] * a.m[1][1])) / det;
  b.m[1][0] = ( - (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0])) / det;
  b.m[1][1] = ( + (a.m[0][0] * a.m[2][2] - a.m[0][2] * a.m[2][0])) / det;
  b.m[1][2] = ( - (a.m[0][0] * a.m[1][2] - a.m[0][2] * a.m[1][0])) / det;
  b.m[2][0] = ( + (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0])) / det;
  b.m[2][1] = ( - (a.m[0][0] * a.m[2][1] - a.m[0][1] * a.m[2][0])) / det;
  b.m[2][2] = ( + (a.m[0][0] * a.m[1][1] - a.m[0][1] * a.m[1][0])) / det;
  return b;
}

/* Check for nan elements. */
int matrix33_isnan(matrix33_t a)
{
  int nan;
  nan = 0;
  nan = nan | isnan(a.m[0][0]);
  nan = nan | isnan(a.m[0][1]);
  nan = nan | isnan(a.m[0][2]);
  nan = nan | isnan(a.m[1][0]);
  nan = nan | isnan(a.m[1][1]);
  nan = nan | isnan(a.m[1][2]);
  nan = nan | isnan(a.m[2][0]);
  nan = nan | isnan(a.m[2][1]);
  nan = nan | isnan(a.m[2][2]);
  return nan;
}

#if defined MATRIX31 && defined MATRIX31
/* Matrix/vector multiplication c = ab. */
matrix31_t matrix33_mul_31(matrix33_t a, matrix31_t b)
{
  matrix31_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
  return c;
}
#endif

#if defined MATRIX32 && defined MATRIX32
/* Matrix/vector multiplication c = ab. */
matrix32_t matrix33_mul_32(matrix33_t a, matrix32_t b)
{
  matrix32_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
  return c;
}
#endif

#if defined MATRIX33 && defined MATRIX33
/* Matrix/vector multiplication c = ab. */
matrix33_t matrix33_mul_33(matrix33_t a, matrix33_t b)
{
  matrix33_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1];
  c.m[0][2] = + a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1];
  c.m[1][2] = + a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1];
  c.m[2][2] = + a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2];
  return c;
}
#endif



/* Set matrix elements. */
matrix44_t matrix44_set(double v00, double v01, double v02, double v03, double v10, double v11, double v12, double v13, double v20, double v21, double v22, double v23, double v30, double v31, double v32, double v33)
{
  matrix44_t a;
  a.m[0][0] = v00;
  a.m[0][1] = v01;
  a.m[0][2] = v02;
  a.m[0][3] = v03;
  a.m[1][0] = v10;
  a.m[1][1] = v11;
  a.m[1][2] = v12;
  a.m[1][3] = v13;
  a.m[2][0] = v20;
  a.m[2][1] = v21;
  a.m[2][2] = v22;
  a.m[2][3] = v23;
  a.m[3][0] = v30;
  a.m[3][1] = v31;
  a.m[3][2] = v32;
  a.m[3][3] = v33;
  return a;
}

/* Set matrix elements. */
matrix44_t matrix44_setv(double v[16])
{
  matrix44_t a;
  a.m[0][0] = v[0];
  a.m[0][1] = v[1];
  a.m[0][2] = v[2];
  a.m[0][3] = v[3];
  a.m[1][0] = v[4];
  a.m[1][1] = v[5];
  a.m[1][2] = v[6];
  a.m[1][3] = v[7];
  a.m[2][0] = v[8];
  a.m[2][1] = v[9];
  a.m[2][2] = v[10];
  a.m[2][3] = v[11];
  a.m[3][0] = v[12];
  a.m[3][1] = v[13];
  a.m[3][2] = v[14];
  a.m[3][3] = v[15];
  return a;
}

/* Construct identity matrix. */
matrix44_t matrix44_ident()
{
  matrix44_t a;
  a.m[0][0] = 1;
  a.m[1][0] = 0;
  a.m[2][0] = 0;
  a.m[3][0] = 0;
  a.m[0][1] = 0;
  a.m[1][1] = 1;
  a.m[2][1] = 0;
  a.m[3][1] = 0;
  a.m[0][2] = 0;
  a.m[1][2] = 0;
  a.m[2][2] = 1;
  a.m[3][2] = 0;
  a.m[0][3] = 0;
  a.m[1][3] = 0;
  a.m[2][3] = 0;
  a.m[3][3] = 1;
  return a;
}

/* Add two matrixs. */
matrix44_t matrix44_add(matrix44_t a, matrix44_t b)
{
  matrix44_t c;
  c.m[0][0] = a.m[0][0] + b.m[0][0];
  c.m[0][1] = a.m[0][1] + b.m[0][1];
  c.m[0][2] = a.m[0][2] + b.m[0][2];
  c.m[0][3] = a.m[0][3] + b.m[0][3];
  c.m[1][0] = a.m[1][0] + b.m[1][0];
  c.m[1][1] = a.m[1][1] + b.m[1][1];
  c.m[1][2] = a.m[1][2] + b.m[1][2];
  c.m[1][3] = a.m[1][3] + b.m[1][3];
  c.m[2][0] = a.m[2][0] + b.m[2][0];
  c.m[2][1] = a.m[2][1] + b.m[2][1];
  c.m[2][2] = a.m[2][2] + b.m[2][2];
  c.m[2][3] = a.m[2][3] + b.m[2][3];
  c.m[3][0] = a.m[3][0] + b.m[3][0];
  c.m[3][1] = a.m[3][1] + b.m[3][1];
  c.m[3][2] = a.m[3][2] + b.m[3][2];
  c.m[3][3] = a.m[3][3] + b.m[3][3];
  return c;
}

/* Add Subtract matrixs: c = a - b. */
matrix44_t matrix44_sub(matrix44_t a, matrix44_t b)
{
  matrix44_t c;
  c.m[0][0] = a.m[0][0] - b.m[0][0];
  c.m[0][1] = a.m[0][1] - b.m[0][1];
  c.m[0][2] = a.m[0][2] - b.m[0][2];
  c.m[0][3] = a.m[0][3] - b.m[0][3];
  c.m[1][0] = a.m[1][0] - b.m[1][0];
  c.m[1][1] = a.m[1][1] - b.m[1][1];
  c.m[1][2] = a.m[1][2] - b.m[1][2];
  c.m[1][3] = a.m[1][3] - b.m[1][3];
  c.m[2][0] = a.m[2][0] - b.m[2][0];
  c.m[2][1] = a.m[2][1] - b.m[2][1];
  c.m[2][2] = a.m[2][2] - b.m[2][2];
  c.m[2][3] = a.m[2][3] - b.m[2][3];
  c.m[3][0] = a.m[3][0] - b.m[3][0];
  c.m[3][1] = a.m[3][1] - b.m[3][1];
  c.m[3][2] = a.m[3][2] - b.m[3][2];
  c.m[3][3] = a.m[3][3] - b.m[3][3];
  return c;
}

/* Matrix multiplication c = ab. */
matrix44_t matrix44_mul(matrix44_t a, matrix44_t b)
{
  matrix44_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0] + a.m[0][3] * b.m[3][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1] + a.m[0][3] * b.m[3][1];
  c.m[0][2] = + a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2] + a.m[0][3] * b.m[3][2];
  c.m[0][3] = + a.m[0][0] * b.m[0][3] + a.m[0][1] * b.m[1][3] + a.m[0][2] * b.m[2][3] + a.m[0][3] * b.m[3][3];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0] + a.m[1][3] * b.m[3][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1] + a.m[1][3] * b.m[3][1];
  c.m[1][2] = + a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2] + a.m[1][3] * b.m[3][2];
  c.m[1][3] = + a.m[1][0] * b.m[0][3] + a.m[1][1] * b.m[1][3] + a.m[1][2] * b.m[2][3] + a.m[1][3] * b.m[3][3];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0] + a.m[2][3] * b.m[3][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1] + a.m[2][3] * b.m[3][1];
  c.m[2][2] = + a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2] + a.m[2][3] * b.m[3][2];
  c.m[2][3] = + a.m[2][0] * b.m[0][3] + a.m[2][1] * b.m[1][3] + a.m[2][2] * b.m[2][3] + a.m[2][3] * b.m[3][3];
  c.m[3][0] = + a.m[3][0] * b.m[0][0] + a.m[3][1] * b.m[1][0] + a.m[3][2] * b.m[2][0] + a.m[3][3] * b.m[3][0];
  c.m[3][1] = + a.m[3][0] * b.m[0][1] + a.m[3][1] * b.m[1][1] + a.m[3][2] * b.m[2][1] + a.m[3][3] * b.m[3][1];
  c.m[3][2] = + a.m[3][0] * b.m[0][2] + a.m[3][1] * b.m[1][2] + a.m[3][2] * b.m[2][2] + a.m[3][3] * b.m[3][2];
  c.m[3][3] = + a.m[3][0] * b.m[0][3] + a.m[3][1] * b.m[1][3] + a.m[3][2] * b.m[2][3] + a.m[3][3] * b.m[3][3];
  return c;
}

#if defined VECTOR4
/** @brief Matrix/vector multiplication c = ab. */
vector4_t matrix44_mul_4(matrix44_t a, vector4_t b)
{
  vector4_t c;
  c.x = + a.m[0][0] * b.x + a.m[0][1] * b.y + a.m[0][2] * b.z + a.m[0][3] * b.w;
  c.y = + a.m[1][0] * b.x + a.m[1][1] * b.y + a.m[1][2] * b.z + a.m[1][3] * b.w;
  c.z = + a.m[2][0] * b.x + a.m[2][1] * b.y + a.m[2][2] * b.z + a.m[2][3] * b.w;
  c.w = + a.m[3][0] * b.x + a.m[3][1] * b.y + a.m[3][2] * b.z + a.m[3][3] * b.w;
  return c;
}
#endif

#if defined MATRIX44
/* Transpose matrix. */
matrix44_t matrix44_trans(matrix44_t a)
{
  matrix44_t b;
  b.m[0][0] = a.m[0][0];
  b.m[1][0] = a.m[0][1];
  b.m[2][0] = a.m[0][2];
  b.m[3][0] = a.m[0][3];
  b.m[0][1] = a.m[1][0];
  b.m[1][1] = a.m[1][1];
  b.m[2][1] = a.m[1][2];
  b.m[3][1] = a.m[1][3];
  b.m[0][2] = a.m[2][0];
  b.m[1][2] = a.m[2][1];
  b.m[2][2] = a.m[2][2];
  b.m[3][2] = a.m[2][3];
  b.m[0][3] = a.m[3][0];
  b.m[1][3] = a.m[3][1];
  b.m[2][3] = a.m[3][2];
  b.m[3][3] = a.m[3][3];
  return b;
}
#endif

/* Invert matrix. */
matrix44_t matrix44_inv(matrix44_t a)
{
  matrix44_t b;
  double det;
  det = ( + a.m[0][0] * ( + a.m[1][1] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[1][2] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) + a.m[1][3] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1])) - a.m[0][1] * ( + a.m[1][0] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[1][2] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[1][3] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0])) + a.m[0][2] * ( + a.m[1][0] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) - a.m[1][1] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[1][3] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0])) - a.m[0][3] * ( + a.m[1][0] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1]) - a.m[1][1] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0]) + a.m[1][2] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0])));
  b.m[0][0] = ( + ( + a.m[1][1] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[1][2] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) + a.m[1][3] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1]))) / det;
  b.m[0][1] = ( - ( + a.m[0][1] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[0][2] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) + a.m[0][3] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1]))) / det;
  b.m[0][2] = ( + ( + a.m[0][1] * (a.m[1][2] * a.m[3][3] - a.m[1][3] * a.m[3][2]) - a.m[0][2] * (a.m[1][1] * a.m[3][3] - a.m[1][3] * a.m[3][1]) + a.m[0][3] * (a.m[1][1] * a.m[3][2] - a.m[1][2] * a.m[3][1]))) / det;
  b.m[0][3] = ( - ( + a.m[0][1] * (a.m[1][2] * a.m[2][3] - a.m[1][3] * a.m[2][2]) - a.m[0][2] * (a.m[1][1] * a.m[2][3] - a.m[1][3] * a.m[2][1]) + a.m[0][3] * (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1]))) / det;
  b.m[1][0] = ( - ( + a.m[1][0] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[1][2] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[1][3] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0]))) / det;
  b.m[1][1] = ( + ( + a.m[0][0] * (a.m[2][2] * a.m[3][3] - a.m[2][3] * a.m[3][2]) - a.m[0][2] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[0][3] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0]))) / det;
  b.m[1][2] = ( - ( + a.m[0][0] * (a.m[1][2] * a.m[3][3] - a.m[1][3] * a.m[3][2]) - a.m[0][2] * (a.m[1][0] * a.m[3][3] - a.m[1][3] * a.m[3][0]) + a.m[0][3] * (a.m[1][0] * a.m[3][2] - a.m[1][2] * a.m[3][0]))) / det;
  b.m[1][3] = ( + ( + a.m[0][0] * (a.m[1][2] * a.m[2][3] - a.m[1][3] * a.m[2][2]) - a.m[0][2] * (a.m[1][0] * a.m[2][3] - a.m[1][3] * a.m[2][0]) + a.m[0][3] * (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0]))) / det;
  b.m[2][0] = ( + ( + a.m[1][0] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) - a.m[1][1] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[1][3] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0]))) / det;
  b.m[2][1] = ( - ( + a.m[0][0] * (a.m[2][1] * a.m[3][3] - a.m[2][3] * a.m[3][1]) - a.m[0][1] * (a.m[2][0] * a.m[3][3] - a.m[2][3] * a.m[3][0]) + a.m[0][3] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0]))) / det;
  b.m[2][2] = ( + ( + a.m[0][0] * (a.m[1][1] * a.m[3][3] - a.m[1][3] * a.m[3][1]) - a.m[0][1] * (a.m[1][0] * a.m[3][3] - a.m[1][3] * a.m[3][0]) + a.m[0][3] * (a.m[1][0] * a.m[3][1] - a.m[1][1] * a.m[3][0]))) / det;
  b.m[2][3] = ( - ( + a.m[0][0] * (a.m[1][1] * a.m[2][3] - a.m[1][3] * a.m[2][1]) - a.m[0][1] * (a.m[1][0] * a.m[2][3] - a.m[1][3] * a.m[2][0]) + a.m[0][3] * (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0]))) / det;
  b.m[3][0] = ( - ( + a.m[1][0] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1]) - a.m[1][1] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0]) + a.m[1][2] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0]))) / det;
  b.m[3][1] = ( + ( + a.m[0][0] * (a.m[2][1] * a.m[3][2] - a.m[2][2] * a.m[3][1]) - a.m[0][1] * (a.m[2][0] * a.m[3][2] - a.m[2][2] * a.m[3][0]) + a.m[0][2] * (a.m[2][0] * a.m[3][1] - a.m[2][1] * a.m[3][0]))) / det;
  b.m[3][2] = ( - ( + a.m[0][0] * (a.m[1][1] * a.m[3][2] - a.m[1][2] * a.m[3][1]) - a.m[0][1] * (a.m[1][0] * a.m[3][2] - a.m[1][2] * a.m[3][0]) + a.m[0][2] * (a.m[1][0] * a.m[3][1] - a.m[1][1] * a.m[3][0]))) / det;
  b.m[3][3] = ( + ( + a.m[0][0] * (a.m[1][1] * a.m[2][2] - a.m[1][2] * a.m[2][1]) - a.m[0][1] * (a.m[1][0] * a.m[2][2] - a.m[1][2] * a.m[2][0]) + a.m[0][2] * (a.m[1][0] * a.m[2][1] - a.m[1][1] * a.m[2][0]))) / det;
  return b;
}

/* Check for nan elements. */
int matrix44_isnan(matrix44_t a)
{
  int nan;
  nan = 0;
  nan = nan | isnan(a.m[0][0]);
  nan = nan | isnan(a.m[0][1]);
  nan = nan | isnan(a.m[0][2]);
  nan = nan | isnan(a.m[0][3]);
  nan = nan | isnan(a.m[1][0]);
  nan = nan | isnan(a.m[1][1]);
  nan = nan | isnan(a.m[1][2]);
  nan = nan | isnan(a.m[1][3]);
  nan = nan | isnan(a.m[2][0]);
  nan = nan | isnan(a.m[2][1]);
  nan = nan | isnan(a.m[2][2]);
  nan = nan | isnan(a.m[2][3]);
  nan = nan | isnan(a.m[3][0]);
  nan = nan | isnan(a.m[3][1]);
  nan = nan | isnan(a.m[3][2]);
  nan = nan | isnan(a.m[3][3]);
  return nan;
}

#if defined MATRIX41 && defined MATRIX41
/* Matrix/vector multiplication c = ab. */
matrix41_t matrix44_mul_41(matrix44_t a, matrix41_t b)
{
  matrix41_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0] + a.m[0][3] * b.m[3][0];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0] + a.m[1][3] * b.m[3][0];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0] + a.m[2][3] * b.m[3][0];
  c.m[3][0] = + a.m[3][0] * b.m[0][0] + a.m[3][1] * b.m[1][0] + a.m[3][2] * b.m[2][0] + a.m[3][3] * b.m[3][0];
  return c;
}
#endif

#if defined MATRIX42 && defined MATRIX42
/* Matrix/vector multiplication c = ab. */
matrix42_t matrix44_mul_42(matrix44_t a, matrix42_t b)
{
  matrix42_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0] + a.m[0][3] * b.m[3][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1] + a.m[0][3] * b.m[3][1];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0] + a.m[1][3] * b.m[3][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1] + a.m[1][3] * b.m[3][1];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0] + a.m[2][3] * b.m[3][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1] + a.m[2][3] * b.m[3][1];
  c.m[3][0] = + a.m[3][0] * b.m[0][0] + a.m[3][1] * b.m[1][0] + a.m[3][2] * b.m[2][0] + a.m[3][3] * b.m[3][0];
  c.m[3][1] = + a.m[3][0] * b.m[0][1] + a.m[3][1] * b.m[1][1] + a.m[3][2] * b.m[2][1] + a.m[3][3] * b.m[3][1];
  return c;
}
#endif

#if defined MATRIX43 && defined MATRIX43
/* Matrix/vector multiplication c = ab. */
matrix43_t matrix44_mul_43(matrix44_t a, matrix43_t b)
{
  matrix43_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0] + a.m[0][3] * b.m[3][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1] + a.m[0][3] * b.m[3][1];
  c.m[0][2] = + a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2] + a.m[0][3] * b.m[3][2];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0] + a.m[1][3] * b.m[3][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1] + a.m[1][3] * b.m[3][1];
  c.m[1][2] = + a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2] + a.m[1][3] * b.m[3][2];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0] + a.m[2][3] * b.m[3][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1] + a.m[2][3] * b.m[3][1];
  c.m[2][2] = + a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2] + a.m[2][3] * b.m[3][2];
  c.m[3][0] = + a.m[3][0] * b.m[0][0] + a.m[3][1] * b.m[1][0] + a.m[3][2] * b.m[2][0] + a.m[3][3] * b.m[3][0];
  c.m[3][1] = + a.m[3][0] * b.m[0][1] + a.m[3][1] * b.m[1][1] + a.m[3][2] * b.m[2][1] + a.m[3][3] * b.m[3][1];
  c.m[3][2] = + a.m[3][0] * b.m[0][2] + a.m[3][1] * b.m[1][2] + a.m[3][2] * b.m[2][2] + a.m[3][3] * b.m[3][2];
  return c;
}
#endif

#if defined MATRIX44 && defined MATRIX44
/* Matrix/vector multiplication c = ab. */
matrix44_t matrix44_mul_44(matrix44_t a, matrix44_t b)
{
  matrix44_t c;
  c.m[0][0] = + a.m[0][0] * b.m[0][0] + a.m[0][1] * b.m[1][0] + a.m[0][2] * b.m[2][0] + a.m[0][3] * b.m[3][0];
  c.m[0][1] = + a.m[0][0] * b.m[0][1] + a.m[0][1] * b.m[1][1] + a.m[0][2] * b.m[2][1] + a.m[0][3] * b.m[3][1];
  c.m[0][2] = + a.m[0][0] * b.m[0][2] + a.m[0][1] * b.m[1][2] + a.m[0][2] * b.m[2][2] + a.m[0][3] * b.m[3][2];
  c.m[0][3] = + a.m[0][0] * b.m[0][3] + a.m[0][1] * b.m[1][3] + a.m[0][2] * b.m[2][3] + a.m[0][3] * b.m[3][3];
  c.m[1][0] = + a.m[1][0] * b.m[0][0] + a.m[1][1] * b.m[1][0] + a.m[1][2] * b.m[2][0] + a.m[1][3] * b.m[3][0];
  c.m[1][1] = + a.m[1][0] * b.m[0][1] + a.m[1][1] * b.m[1][1] + a.m[1][2] * b.m[2][1] + a.m[1][3] * b.m[3][1];
  c.m[1][2] = + a.m[1][0] * b.m[0][2] + a.m[1][1] * b.m[1][2] + a.m[1][2] * b.m[2][2] + a.m[1][3] * b.m[3][2];
  c.m[1][3] = + a.m[1][0] * b.m[0][3] + a.m[1][1] * b.m[1][3] + a.m[1][2] * b.m[2][3] + a.m[1][3] * b.m[3][3];
  c.m[2][0] = + a.m[2][0] * b.m[0][0] + a.m[2][1] * b.m[1][0] + a.m[2][2] * b.m[2][0] + a.m[2][3] * b.m[3][0];
  c.m[2][1] = + a.m[2][0] * b.m[0][1] + a.m[2][1] * b.m[1][1] + a.m[2][2] * b.m[2][1] + a.m[2][3] * b.m[3][1];
  c.m[2][2] = + a.m[2][0] * b.m[0][2] + a.m[2][1] * b.m[1][2] + a.m[2][2] * b.m[2][2] + a.m[2][3] * b.m[3][2];
  c.m[2][3] = + a.m[2][0] * b.m[0][3] + a.m[2][1] * b.m[1][3] + a.m[2][2] * b.m[2][3] + a.m[2][3] * b.m[3][3];
  c.m[3][0] = + a.m[3][0] * b.m[0][0] + a.m[3][1] * b.m[1][0] + a.m[3][2] * b.m[2][0] + a.m[3][3] * b.m[3][0];
  c.m[3][1] = + a.m[3][0] * b.m[0][1] + a.m[3][1] * b.m[1][1] + a.m[3][2] * b.m[2][1] + a.m[3][3] * b.m[3][1];
  c.m[3][2] = + a.m[3][0] * b.m[0][2] + a.m[3][1] * b.m[1][2] + a.m[3][2] * b.m[2][2] + a.m[3][3] * b.m[3][2];
  c.m[3][3] = + a.m[3][0] * b.m[0][3] + a.m[3][1] * b.m[1][3] + a.m[3][2] * b.m[2][3] + a.m[3][3] * b.m[3][3];
  return c;
}
#endif



/* Set pose elements. */
pose2_t pose2_set(vector2_t pos, double rot)
{
  pose2_t a;
  a.pos = pos;
  a.rot = rot;
  return a;
}

/* Take pose "inverse"*/
pose2_t pose2_inv(pose2_t a)
{
  pose2_t b, c;
  b.pos.x = 0; b.pos.y = 0; b.rot = 0;
  c = pose2_sub(b, a);
  return c;
}

/* Add two poses. */
pose2_t pose2_add(pose2_t a, pose2_t b)
{
  pose2_t c;
  c.pos = pose2_add_pos(a.pos, b);
  c.rot = pose2_add_rot(a.rot, b);
  return c;
}

/* Add two poses. */
vector2_t pose2_add_pos(vector2_t a, pose2_t b)
{
  double co, si;  vector2_t c;
  co = cos(b.rot); si = sin(b.rot);
  c.x = co * a.x - si * a.y + b.pos.x;
  c.y = si * a.x + co * a.y + b.pos.y;
  return c;
}

/* Add two poses (rotation component). */
double pose2_add_rot(double a, pose2_t b)
{
  double c;
  c = atan2(sin(a + b.rot), cos(a + b.rot));
  return c;
}

/* Subtract poses: c = a - b. */
pose2_t pose2_sub(pose2_t a, pose2_t b)
{
  pose2_t c;
  c.pos = pose2_sub_pos(a.pos, b);
  c.rot = pose2_sub_rot(a.rot, b);
  return c;
}

/* Subtract two poses. */
vector2_t pose2_sub_pos(vector2_t a, pose2_t b)
{
  double co, si;
  vector2_t c;
  co = cos(b.rot); si = sin(b.rot);
  c.x = +co * (a.x - b.pos.x) + si * (a.y - b.pos.y);
  c.y = -si * (a.x - b.pos.x) + co * (a.y - b.pos.y);
  return c;
}

/* Subtract two poses (rotation component). */
double pose2_sub_rot(double a, pose2_t b)
{
  double c;
  c = atan2(sin(a - b.rot), cos(a - b.rot));
  return c;
}



